/* Includes the Standard IO Library */
#include <stdio.h>
#include <math.h>
/*
 *  roverSide.h
 *  CommandCenter08
 *
 *  Created by David P. Miller on 7/22/08.
 *  Copyright 2008 University of Oklahoma. All rights reserved.
 *
 */

//All of the structs are defined in definitions.h
#include "definitions.h"

#include "obstacleMap.h"

//**************** Function Prototypes ********

// Get an angle between 0 and 360 degrees
float normalizeDegAngle(float a);

// Get an angle between 0 and 2*pi
float normalizeRadAngle(float a);

//Finds the average angle in radians of two angles in degrees
//Presumes the two headings make an acute angle
//Converts angle from compass degrees (CW, N=0) to
//XY angle (CCW, E=0)
float avgHeadingToDegrees(float deg1, float deg2);

//find the distance in the XY plane of two points
float distanceBetweenPoints(Point3D p1, Point3D p2);

// takes pointer to rover position and the current values for encoders and compass
// and updates the rover structure with those new values and the new position as
// calculated from those values and the rover's previous position.
void updateRoverPosition (roverProperties *rover, int cltics, int crtics, float ccompass);

// Calculates the compass heading to the waypoint from the rover's current position
float headHereToThere(roverProperties *rover);

// returns a turning angle from -180 to 180 in order to head
// towards the current waypoint
float turnAngleTowardsThere(roverProperties *rover);

// returns the distance between rover and way point
float distanceHereToThere(roverProperties *rover);

// stops rover and says error message; data is one value of data (e.g., overlarge update dist
void callForHelp(roverProperties *rover, int errCode, int data);

// gets the rover turning the requested amount (large turns are done as point turns. Small turns done gradually).
// this function just sets the speeds and returns. relHead is in compass format degrees
void turnToHeading(roverProperties *rover, float inHead);

//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
//***********************************************************************************
// HIGH LEVEL FUNCTION CALL
// Moves robot towards waypoint.  When WP is reached it stops rover and returns 0, otherwise
// the distance to the waypoint is returned
float moveToWaypoint(roverProperties *rover);
//***********************************************************************************

// there is an obstacle nearby, so stop, backup if needed, turn turnAvoidDirection to avoid the obstacle
// then drive past the obstacle.  When that is completed turn rover state back to normal
void avoidNearbyObstacle(roverProperties *rover) ;

// there is not an obstacle within 1m and there is an obstacle 1-4m away and the upcoming waypoint is past
// that distance, so turn the rover turnAvoidDirection to go to the side of the upcoming obstacle. When
// the rover is past the upcoming obstacle, switch the rover state back to normal
void avoidDistantObstacle(roverProperties *rover) ;

// Is there an obstacle about to be hit?
void checkForImmediateObstacles(roverProperties *rover);

// Is there an obstacle more than 1m but closer than the WP?
float checkForDistantOsbtacles(roverProperties *rover, float wpDistance);

// take evasive action
void avoidingTurn(roverProperties *rover, float obstacleDirection, int lastObstacleTurnDir);


// Get the distance and a heading that avoids obstacle from the body laser
void obstacleDistanceAndClearHeading(roverProperties *rover, float *obstacleDistance, float *clearHeading);

// profile laser generates 16 values which cover 90 degreees of angle or 5.625 degrees each
// this function converts those values into height relative to the rover assuming the rover is on flat
//ground.  The function then returns either a -1, 0 or 1 depending on whether it detects a
// significant negative obstacle, flat ground or positive obstacle.
// the distance to the begining of the obstacle is placed in obstDist
//deviation is filled by this function
// The profile laser is only used 1.4m in front of the rover
int terrainProfile(roverProperties *rover, float *obstDist, float deviation[], int *proIdx);

//returns 1 if there is not a real obstacle at about dist and 0 otherwise
float profileNoPosObstacle(roverProperties *rover);

//takes an array of at least PROFILESEG in length and fills the first PRFOFILESEG elements with the heights of the surface
void getProfileHeights(roverProperties *rover, float heights[]);

//takes an array of at least PANELSEG and fills it with the heights from a flat surface.  Note that since the measurements
//are the shortest possible ray paths, the two center segments are calculated as if they are both at an angle 0
//also note that the panel scanner is mounted at 45 degrees, hence the SQRTTWO fact in the height calculation
void getPanelHeights(roverProperties *rover, float heights[]);

